/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Robert Bosch LLC.
*  Copyright (c) 2015-2016, Jiri Horner.
*  Copyright (c) 2021, Carlos Alvarez, Juan Galvis.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Jiri Horner nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/

#include <explore/explore.h>

#include <thread>

inline static bool same_point(const geometry_msgs::msg::Pose &one,
                          const geometry_msgs::msg::Pose &two)
{
    double dx = one.position.x - two.position.x;
    double dy = one.position.y - two.position.y;
    double dist = sqrt(dx * dx + dy * dy);
    return dist < 0.05;
}

inline static double two_point_dist(
        const geometry_msgs::msg::Pose &one,
        const geometry_msgs::msg::Point &two)
{
    double dx = one.position.x - two.x;
    double dy = one.position.y - two.y;
    return sqrt(dx * dx + dy * dy);
}

namespace explore
{
Explore::Explore()
        : Node("explore_node"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_),
          costmap_client_(*this, &tf_buffer_), prev_distance_(0), last_markers_count_(0)
{
    double timeout;
    double min_frontier_size;
    this->declare_parameter<float>("planner_frequency", 1.0);
    this->declare_parameter<float>("progress_timeout", 30.0);
    this->declare_parameter<bool>("visualize", true);
    this->declare_parameter<float>("potential_scale", 1e-3);
    this->declare_parameter<float>("gain_scale", 1.0);
    this->declare_parameter<float>("min_frontier_size", 1.0);
    this->declare_parameter<bool>("return_to_init", true);

    this->get_parameter("planner_frequency", planner_frequency_);
    this->get_parameter("progress_timeout", timeout);
    this->get_parameter("visualize", visualize_);
    this->get_parameter("potential_scale", potential_scale_);
    this->get_parameter("gain_scale", gain_scale_);
    this->get_parameter("min_frontier_size", min_frontier_size);
    this->get_parameter("return_to_init", return_to_init_);
    this->get_parameter("robot_base_frame", robot_base_frame_);

    progress_timeout_ = timeout;
    move_base_client_ =
            rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
                    this, ACTION_NAME);

    search_ = frontier_exploration::FrontierSearch(
            costmap_client_.getCostmap(), potential_scale_, gain_scale_, min_frontier_size);
    target_position.x = 0;
    target_position.y = 0;
    target_prev.position.x = 0;
    target_prev.position.y = 0;
    if (visualize_)
    {
        marker_array_publisher_ =
                this->create_publisher<visualization_msgs::msg::MarkerArray>("explore/frontiers", 10);
    }
    // Subscription to resume or stop exploration
    resume_subscription_ = this->create_subscription<std_msgs::msg::Bool>(
            "explore/resume", 10,
            std::bind(&Explore::resumeCallback, this, std::placeholders::_1));
    auto qos = rclcpp::QoS(rclcpp::KeepLast(1));
    auto cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", qos);
    RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server");
    move_base_client_->wait_for_action_server();
    RCLCPP_INFO(logger_, "Connected to move_base nav2 server");
    if (return_to_init_)
    {
        RCLCPP_INFO(logger_, "Getting initial pose of the robot");
        geometry_msgs::msg::TransformStamped transformStamped;
        std::string map_frame = costmap_client_.getGlobalFrameID();
        try
        {
            transformStamped = tf_buffer_.lookupTransform(
                    map_frame, robot_base_frame_, tf2::TimePointZero);
            initial_pose_.position.x = transformStamped.transform.translation.x;
            initial_pose_.position.y = transformStamped.transform.translation.y;
            initial_pose_.orientation = transformStamped.transform.rotation;
        } catch (tf2::TransformException &ex)
        {
            RCLCPP_ERROR(logger_, "Couldn't find transform from %s to %s: %s",
                         map_frame.c_str(), robot_base_frame_.c_str(), ex.what());
            return_to_init_ = false;
        }
    }
    exploring_timer_ = this->create_wall_timer(
            std::chrono::milliseconds((uint16_t) (1000.0 / planner_frequency_)),
            [this]() { makePlan(); });
    // Start exploration right away
    exploring_timer_->execute_callback();
    geometry_msgs::msg::Twist cmd_vel;
    cmd_vel.angular.z = 1;
    rclcpp::Rate loop_rate(10);
    int cnt = 0;
    while (cnt < 10)
    {
        cmd_vel_pub_->publish(cmd_vel);
        loop_rate.sleep();
        cnt++;
    }
}

Explore::~Explore() { stop(); }

void Explore::resumeCallback(const std_msgs::msg::Bool::SharedPtr msg)
{
    if (msg->data) resume();
    else stop();
}

void Explore::visualizeFrontiers(
        const std::vector<frontier_exploration::Frontier> &frontiers)
{
    std_msgs::msg::ColorRGBA green;
    green.r = 0;
    green.g = 1.0;
    green.b = 0;
    green.a = 1.0;

    RCLCPP_DEBUG(logger_, "visualising %lu frontiers", frontiers.size());

    visualization_msgs::msg::MarkerArray room_sequence_visualization_msg_;
    visualization_msgs::msg::Marker circle;
    circle.action = visualization_msgs::msg::Marker::DELETEALL;
    room_sequence_visualization_msg_.markers.push_back(circle);
    marker_array_publisher_->publish(room_sequence_visualization_msg_);

    size_t id = 0;
    for (auto &frontier : frontiers)
    {
        circle.header.frame_id = "/map";
        circle.header.stamp = rclcpp::Time();
        circle.id = int(id);
        circle.type = visualization_msgs::msg::Marker::SPHERE;
        circle.action = visualization_msgs::msg::Marker::ADD;
        circle.pose.position = frontier.centroid;
        circle.pose.orientation.w = 1.0;
        circle.scale.x = 0.20;        // this is the line width
        circle.scale.y = 0.20;
        circle.scale.z = 0.5;
        circle.color = green;
        circle.lifetime = rclcpp::Duration(rclcpp::Duration::from_seconds(0));
        room_sequence_visualization_msg_.markers.push_back(circle);
        ++id;
    }
    marker_array_publisher_->publish(room_sequence_visualization_msg_);
}

void Explore::makePlan()
{
   
    // find frontiers
    auto pose = costmap_client_.getRobotPose();
    double dist = two_point_dist(pose, target_position);
    isMakePlan = fabs(dist) < 0.5 && fabs(dist) > 0;
    if (fabs(dist) < 0.5 && fabs(dist) > 0)
    {
    	isMakePlan = true;
    	noReachTargetPt.push_back(frontierEnd);
    }
    else isMakePlan = false;
    if (same_point(target_prev, pose))same_point_cnt++;
    else same_point_cnt = 0;
    if (same_point_cnt > 6)
    {
        noReachTargetPt.push_back(frontierEnd);
        isMakePlan = true;
    }
    std::cout << "pose x: " << pose.position.x << " y: " << pose.position.y
              << " target_position x: " << target_position.x << " y: " << target_position.y
              << " target_prev x: " << target_prev.position.x << " y: " << target_prev.position.y
              << " dist: " << dist << " same_point_cnt: " << same_point_cnt
              << " isMakePlan: " << isMakePlan << std::endl;
    target_prev = pose;
    if (!isMakePlan)return;
    // get frontiers sorted according to cost
    auto frontiers = search_.searchFrom(pose.position);
    for (const auto& pt:noReachTargetPt)
    {
        for (auto iter = frontiers.end(); iter != frontiers.begin(); iter--)
        {
            if (iter->centroid == pt.centroid)frontiers.erase(iter);
        }
    }
    std::cout << "frontiers: " << frontiers.size() << std::endl;
    if (frontiers.empty())
    {
        if (isStartExplore && !is_return_to_init)
        {
            isMakePlan = true;
            return;
        }
        else
        {
            std::cout << "No frontiers found, stopping." << std::endl;
            stop(true);
            return;
        }
    }
    else isStartExplore = false;
    for (auto fr: frontiers)
    {
        std::cout << "frontier min_distance: " << fr.min_distance
                  << " size: " << fr.size
                  << " cost: " << fr.cost
                  << " initial: " << fr.initial.x
                  << " , " << fr.initial.y
                  << " middle: " << fr.middle.x
                  << " , " << fr.middle.y
                  << " centroid: " << fr.centroid.x
                  << " , " << fr.centroid.y
                  << std::endl;
    }
    // publish frontiers as visualization markers
    if (visualize_) visualizeFrontiers(frontiers);

    frontierEnd = frontiers.back();
    target_position = frontierEnd.centroid;
    // ensure only first call of makePlan was set resuming to true
    if (resuming_) resuming_ = false;
    // we don't need to do anything if we still pursuing the same goal
    std::cout << "target_position.x: " << target_position.x
              << " y: " << target_position.y
              << " z: " << target_position.z
              << std::endl;
    // send goal to move_base if we have something new to pursue
    auto goal = nav2_msgs::action::NavigateToPose::Goal();
    goal.pose.pose.position = target_position;
    goal.pose.pose.orientation.w = 1.;
    goal.pose.header.frame_id = costmap_client_.getGlobalFrameID();
    goal.pose.header.stamp = this->now();
    auto send_goal_options =
            rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
    send_goal_options.result_callback =
            [this](const NavigationGoalHandle::WrappedResult &result)
            {
                reachedGoal(result, target_position);
            };
    isMakePlan = false;
    std::cout << "--------- async_send_goal ---------" << std::endl;
    move_base_client_->async_send_goal(goal, send_goal_options);
    noReachTargetPt.clear();
}

void Explore::returnToInitialPose()
{
    RCLCPP_INFO(logger_, "Returning to initial pose.");
    is_return_to_init = true;
    auto goal = nav2_msgs::action::NavigateToPose::Goal();
    goal.pose.pose.position = initial_pose_.position;
    goal.pose.pose.orientation = initial_pose_.orientation;
    goal.pose.header.frame_id = costmap_client_.getGlobalFrameID();
    goal.pose.header.stamp = this->now();
    auto send_goal_options =
            rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
    move_base_client_->async_send_goal(goal, send_goal_options);
}

bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point &goal)
{
    constexpr static size_t tolerace = 5;
    nav2_costmap_2d::Costmap2D *costmap2d = costmap_client_.getCostmap();

    // check if a goal is on the blacklist for goals that we're pursuing
    for (auto &frontier_goal : frontier_blacklist_)
    {
        double x_diff = fabs(goal.x - frontier_goal.x);
        double y_diff = fabs(goal.y - frontier_goal.y);

        if (x_diff < tolerace * costmap2d->getResolution() &&
            y_diff < tolerace * costmap2d->getResolution())
            return true;
    }
    return false;
}

void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult &result,
                          const geometry_msgs::msg::Point &frontier_goal)
{
    switch (result.code)
    {
        case rclcpp_action::ResultCode::SUCCEEDED:
            std::cout << "Goal was successful" << std::endl;
        case rclcpp_action::ResultCode::ABORTED:
            std::cout << "Goal was aborted, Adding current goal to black list" << std::endl;
//            frontier_blacklist_.push_back(frontier_goal);
            // If it was aborted probably because we've found another frontier goal,
            // so just return and don't make plan again
        case rclcpp_action::ResultCode::CANCELED:
            std::cout << "Goal was canceled" << std::endl;
            // If goal canceled might be because exploration stopped from topic. Don't make new plan.
        default:
            std::cout << "Unknown result code from move base nav2" << std::endl;
    }
    isMakePlan = true;
    if (is_return_to_init) is_return_to_init = false;
    std::cout << "reachedGoal isMakePlan: " << isMakePlan << std::endl;
}

void Explore::start()
{
    RCLCPP_INFO(logger_, "Exploration started.");
}

void Explore::stop(bool finished_exploring)
{
    RCLCPP_INFO(logger_, "Exploration stopped.");
    if (!is_return_to_init)
    {
        move_base_client_->async_cancel_all_goals();
    	exploring_timer_->cancel();
    }
    if (return_to_init_ && finished_exploring)
    {
        returnToInitialPose();
    }
}

void Explore::resume()
{
    resuming_ = true;
    RCLCPP_INFO(logger_, "Exploration resuming.");
    // Reactivate the timer
    exploring_timer_->reset();
    // Resume immediately
    exploring_timer_->execute_callback();
}

}  // namespace explore

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<explore::Explore>());
    rclcpp::shutdown();
    return 0;
}
